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abstract 

An optimal control formulation of the problem of collision avoidance of mobile robots in 
environments containing moving obstacles is presented. Collision avoidance is guaranteed if 
the minimum distance between the robot and the objects is nonzero. A nominal trajectory 
; is assumed to be known from off-line planning. The main idea is to change the velocity 
along the nominal trajectory so that collisions are avoided. Furthermore, time consistency 
f with the nominal plan is desirable. A numerical solution of the optimization problem is 
obtained. Simulation results verify the value of the proposed strategy. 
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1 Introduction 

The problem of “moving a mobile robot or a manipulator from an initial position of the 
workspace to a final one, avoiding the stationary obstacles of the environment and opti- 
mizing over certain criteria while satisfying the kinematic and dynamic constraints of the 
robot”, is well known to be formidable due to its NP-complete computational complexity 
[Can86]. 

Undoubtedly, the more complex version of the problem where moving obstacles are 
present (figure 1) in the environment is even more difficult and computationally expensive. 
The problem of treating moving objects has been stated as early as 1984 [FH84] and usually 
ad hoc solutions were given [Le89] [Tou86]. Reif and Sharir [RS85] gave an algorithmic 
solution to the problem but they were restricted to some categories of shapes of objects, 
and their approach is not suitable for an on-line implementation. On the other hand, Kant 
and Zucker [KZ84], [KZ86], [KZ88], used the decomposition of the motion planning problem 
to the find-path , and move-along-path problems, they propose that the avoidance of moving 
obstacles can be done by adjusting the motion along the geometric path. The same approach 
was adopted in [WJ88], and recently in [GE90]. The basic idea of this approach is utilized 
in this work. Our scheme is more general and complete in the sense that the dynamic 
model of the robot is used, the objects are modeled as convex polyhedra and, in addition to 
collision avoidance, time consistency with the nominal plan is sought. Lately search based 
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Figure 1: Environment with a mobile robot and multiple moving obstacles 

approaches for solving the above problem have also been presented in [FS90],[SLG90]. 

To facilitate a fast solution a hierarchical decomposition has been proposed [KZ88], and 
adopted and extended in our work. The problem is divided in two: 

• Off-line Path and Motion Planning, and 

• On-line Motion Replanning. 

In the off-line stage, two problems are solved: First, path planning, the “find path” 
problem, ie the search for a connected curve r(s) = [pr(s) Py^)] 7 * and an associated ori- 
entation function 0(s), where s is the trajectory parameterization variable, in the robot’s 
workspace, connecting the initial and target points without colliding with the stationary 
objects while satisfying certain criteria. Second, motion planning, the problem of how to 
move along this path without violating the kinematic and dynamic constraints of the robot, 
and some performance criterion (e.g time) is minimized. 

The subject of this paper is the development of an algorithm for the on-line stage. 
This algorithm has to act in a supervisory mode during motion execution in order to avoid 
collisions with moving obstacles with which collision is predicted based on sensory input. 
The new plan must satisfy the dynamic constraints and stay as close as possible to the 
nominal plan. 

In our previous developments [KS90b] [KS91] the Minimum Interference Strategy (MIS) 
was proposed to give fast, feasible and close to optimal solutions. In this work, the Optimal 
Control Strategy (OCS) is proposed to give optimal solutions of the problem at hands. 
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In section 2 the preliminary definitions and the problem are stated. The theoretical 
analysis and the presentation of OCS is done in section 3. Finally, in section 4 simulation 
results and suggestions for future research are presented. 

2 Definitions - Assumptions - Modeling 

Collision avoidance is guaranteed if the distance d(s,t) [GJ85] between the robot and the 
object is greater than a safety positive constant i.e 

d(s,t) = min{||z; - Zj\\ : z; E C r {s),z 3 E C 0 (t)} > d° (1) 


where 

C T (s ) = {x/x A r • R~ 1 (s) -x < br-Ar-R-\s)-T r {s), A r e St mx3 ,6 r e K m }, (2) 

Co(t ) = {y/y G 5? 3 9 A 0 ■ R~\t) • y < b 0 - A 0 • R~\t) ■ T 0 (t), A 0 € ^ x3 ,6 0 € St 1 }, (3) 

are convex polyhedra representing the convex hulls of the mobile robot and the mov- 
ing obstacle respectively. ( A r ,b r ) and ( A 0j b o ) are the parameters that define the convex 
polyhedron description of the robot and the object respectively, with respect to their fixed 
coordinate frame. 7? r ,i? 0 ,T r ,T 0 represent the rotation and translation of the frames of the 
robot and the object with respect to the world frame. A computationally efficient approach 
to estimate d(s,t) and predict the collision time t c under uncertain input from sensing 
devices is presented elsewhere [KS90a]. The collision time is defined by 


tc 


inf 

*€[<o,«o+7h] 


{t/d(t) = 0} 


(4) 


where to is the present time, and T \ is the time horizon of interest. 

The proposed approach works if the following assumption is satisfied. 

Temporary Obstruction Assumption: The mobile robot moving along path r(s) 
can only be obstructed during a bounded amount of time i.e the moving object is assumed 
not to permanently stay on, or move parallel to r(^). 

The problem of planning the motion of the mobile robot such that it avoids the moving 
obstacles while minimizing deviations from the nominal final time can be posed as an 
optimization problem. The mathematical statement of the problem and the derivations of 
the dynamic model and the constraints has been presented in [KS90b] [KS91]. 

System 

x\s ) = A(ar(s)) + Z?(z(s)) • t/(s) (5) 

where (•)' = x(s) = [<(s) t>0)] T , with v{$) = 


A(ar) 




( 6 ) 
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and 


B(x)= ! 0 ! 

. «/(*) . 

Initial-Final Conditions 

(7) 

x(s 0 ) = [0 v 0 ] T x(sj) = [free vj] T 

( 8 ) 

input constraints 

— U 2 < u{s) < U\ 

(9) 

State Constraints 

o < e < v(s) < v 3tab (s ) 

(10) 

Collision Avoidance Constraints 

do - d(s. t) < 0 

(11) 

Performance Criterion 

i = 5 (<(»/) - rf 

(12) 

where s is the trajectory parameterization variable, /(6),u(6) are the time and the velocity 
when mobile robot is at point s of the trajectory. I is the moment of inertia around the 
center of translation, f(s) is the curvature of the cartesian trajectory ^( 5 ), m is the mass of 
the mobile robot and u is the input (force) which is bounded. The velocity u(6) is bounded 
at every instant to guarantee the nonslipping conditions [K.K91]. T is the final time of the 
nominal plan (i.e t n (sj) = T ). Notice that the criterion is simply convex, but not strictly. 


This means that the optimal solution is not necessarily unique. 

3 Optimal Control Strategy (OCS) 

The problem is posed as an optimal control problem with constraints on the states (hard) 
and the control (soft). The analytic solution of such problems is a formidable problem. Thus 
a numerical solution is sought. However in order to investigate the convergence performance 
of such an algorithm, the properties of the feasible control space should be investigated. 

3.1 Feasible Controls Space 

The feasible controls space is defined as 

£/ = j u G Z/ 2 (0, 6/, 3?)/ 3 (8 — 11) are satisfied^ (13) 

Its convexity properties should be investigated for numerical convergence purposes. 

Obviously (9) is convex since for all inputs ui,u 2 G i 2 (0,s/, Jf) that are bounded (i.e 
U\ (<j), u 2 (tr ) G [— a G [0,6/]) it is obvious that its convex combination u = A • u\ + 
(1 — A) * u 2 is bounded (i.e u(ct) G [—E/ 2 , U\]. a G [0,6/]). 
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The final equality constraint (8) can be imposed using inequalities. Consider as upper 
and lower velocity bounds the two velocity arcs starting from and going backwards 

with input — U 2 , U\ until they meet = £ respectively. Thus u m ax(^) and v m ; n (.s) 

are constructed. 

This is demonstrated in figure 2. Thus, a new set of inequalities 

0 ^ ^ — ^max(^) (14) 


is used to satisfy (8,10). Due to the assumption about motion on a horizontal plane, the 
total energy £ of the mobile robot at every instant, is equal to its Kinematic energy. Thus, 

£(*>,«) = \ ( m + 1 • fi s )) - v2 oo ( is ) 

where f(s) is the curvature of the path r(s). Integrating the second of the state equations 
the kinematic energy at a point s is given by 

1 (m + / • / 2 (s)) • u 2 (s) = 1 (m + / • / 2 (s 0 )) • v%(s 0 ) + u(a) ■ da (16) 


where u(a) 6 L 2 (0,sj,$t). Thus, 


v 2 {s) 


(m + 7 • / 2 (sq)) 2 . , 2 // n u(a) ■ da 

0 m + I.p(s )) ' 01 0) ^ (m + I ■ P(s)) 


(17) 
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If velocities ui(s),V 2 (s) 


2 (m + /-/ 2 (s 0 )) 2 

l( ( m + /./2( s )) (m + I-p(s)) 

(18) 

2/ , ( m+I-f 2 (s 0 )) 2/ ^ , 2/; o « 2 (c7).d<7 

V2(S) (m-b/-P(s)) o( o)+ (m + I-p(s)) 

(19) 

satisfy (14), then velocity u(s) 


2 , x (m + i-p{s 0 )) 2/ x, 2 n 0 <°)- da 

(m + /*/ 2 (s)) (m + J-/ 2 (s)) 

(20) 

where u(<r) = A • u\(cr) + (1 — A) • u 2 (a ) a 6 [0, s], satisfies 


f 2 (s) = A • Vi(s) + (1 - A) • v 2 (s) 

(21) 

an therefore it satisfies (14). Therefore (14) is convex. 
Consider the sets 

D = {(s,t)/d(s,t)-d°< 0} 

(22) 

D s = 9 ( 5,0 e £>} 

(23) 

For a specific s, the Collision Times Set (time instants where collision is 
robot is at position s) is defined as 

possible if the 

C t (s) = {t/3(s,t)eD} 

(24) 

Since d(s,t) is continuous w.r.t both arguments, it can be easily proved that if a pair ( s c ,t c ) 
exists such that d(s c ,t c ) = 0 then C*($) is found to be non-empty and compact Vs E D s . 
Thus the following definitions are meaningful 

£ 

Oil 

•♦0 

(25) 

t u (s) = maxCt(s) 

(26) 

Notice that 

t(s) > t u (s)Vs e D s 1 

or > => do — d(s,t) < 0 

(27) 


t(s) < ti(s) Vs e D s J 

showing the sufficiency of those collision avoidance conditions. However, the necessity is 
not true in general. This is demonstrated in figure 3(b). There it is obvious that C*(so) = 
{t/t < ti V t\ <t < t 2 Vt> t u }. 

For safety purposes we are not interested in the interval [ti y t 2 ]* Furthermore, in practice, 
the conic section type of obstacle trajectories that result from the used estimation algorithm 
[KS90a] will not give such intervals. Thus, it becomes obvious that the feasible space is 


6 




Figure 3: Examples of Motion of Obstacles 


( 


decomposed in terms of the collision avoidance constraint in two: the first is the space of 
decelerating plans where 


t(s ) > t u {s) Vs £ D, 
and the second is space of accelerating plans where 

(28) 

t(s) < ti{- s) Vs £ D 3 

(29) 


Therefore we will seek solutions of the optimal control problem in the following feasible 
control spaces: 

Deceleration Space 

Udcc == {u E L 2 (0,5/,3?)/ 9 (9,14,28) are satisfied j (30) 

Acceleration Space 

Uacc = {u 6 Z 2 (0, «/,»)/ 9 (9,14,29) are satisfied} (31) 

Consider the following lemma: 

Lemma .1 If 

a) f(<r),g(<r) € C[0,1], 

b) f(a),g(a) > 0 Vtr £ [0, 1] 

c) f,g not effectively proportional and 

d) s E [0, 1] fixed, 
then M( A) : [0, 1] — ^ 3f? defined by: 

a f 9 1 

M{ A)= / 7=== =— da 

J o \/A * / + (1 — A) * 5 

15 convex w.r.t. its argument A. 

Proof: The proof is given in appendix.|. 

An input ^1(5) E £/ a cc results to ^1(5) such that 

*i( 5 ) = f ’ —7 -r-da < t,(s) 

J0 Vi{(T) 

and U 2 (s) £ U ac c results to V 2 (s) such that 

Their convex combination u(a) = A-«i(< 7) + (1 - A)-u 2 (<0 results to v(s) satisfying (21) i.e 

u(s) = ^A-t?J(s) + (l- A)-u|(s) (35) 

Because of Lemma .1 it is deduced that 

t(s) = [ f —fr ■ da < min{*i(s),< 2 (s)} <(s) < *j( 5 ) (36) 

JO V{<7) 

Therefore U acc is convex. 

Efforts to prove the same for U^ tc have not led to any result. 


(32) 

(33) 

(34) 
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3.2 Numerical Implementation 

A closed form solution of the optimal control problem at hands is a formidable task. There- 
fore a numerical solution was sought. The nonlinear hard constraints (11,14) that can be 
written in the vector form 


G(x(s),s) > 0 G(.,.) G Si 3 (37) 

complicate the numerical solution. The approach of [Mar73] was used to approximate them 
with equivalent soft constraints #(.,.,.) G Sf 3 that satisfy 

^(x(s), u(s),s) = C(x,s) + D(x,s) • u(s) > G(x(s),s) >0 Vs > sq (3S) 


In appendix the approach of [Mar73] is presented. 

If we incorporate the control input constraints into </(.,.,.) then .) G 

To enhance numerical convergence the penalty functions approach was adopted. Thus, 
criterion (12) was changed to 


Jt =k‘M-r?+ r 

& ^50 



Cj(x(5), s) + v } (x(s), s) • u(s) 


• ds 


(39) 


where Tj j = 1,...,5 are very small positive constants. 

Several attempts to solve the problem with shooting and matching techniques proved to 
be futile. The only approach that proved to be successful was the Nonlinear Program- 
ming approach. 

The numerical solution of the above problem requires the descretization of the domain 
[0,5/] in N steps of uniform length As = ^.creating a grid of N + 1 points {0, Si, ..., sjy — 
5/}. The corresponding N control inputs N time instants ...,tf/v) 

and N velocities {vi,v 2 , ..., v/v}, where U{ = v(s = i-As), = t(s = i-As), V{ = v(s = i-As), 
are the unknowns of the problem. 

The nonlinear programming problem is stated, in the form: 


min *e» 3N 

S.t l z < X < T x 

A ■ x = 0 (40) 

c t (x) = 0 
q(x) < 0 


where 


x = [uqj ^ l, — ? V2,...,rjv] r * s ^e vector of unknowns (Notice that the 
velocity components of this vector are the squares of the velocities. This is going to be 
justified later), and 

F( x ) = i( <Ar “ + Silo 1 (^i=i T i ’ c,(r(sO.*0+P>(*( a 0. 4 0-“(*i)) ‘ ( s ‘ +1 ~ 

where T is the total time in the nominal plan. 
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The bounds of the vector of unknowns x are : 

For ii,: 

-U 2 <u,<Ui / = 0,l,...,iV-l 

which actually is constraint (9). 

For vf: 

*4m( S <) < V i < »Li( S >) * = !- —> N 

representing (14). 

The linear equality constraints (.4 • x = 0) are 


(42) 


(m + I • j\ Si )) v] — (m + I ■ / 2 (s,_ i)) • t>?_i - 2 • («,- - s,_i) • m = 0 i = 1, 2V (43) 

representing the dynamics of the system as given by the second state equation in (5). Notice 
that because we use v*f in the vector of unknowns x, this is a linear constraint otherwise, it 
would be a nonlinear one. 

The nonlinear equality constraints (c*(x) = 0) are 


^*+1 



N- 1 


representing the first of the state equations (5). 

Finally, the nonlinear inequality constraints (c^(x) < 0) are 


(44) 


do - d(si,ti ) <0 i = 1,...,JV 


(45) 


which actually represent the collision avoidance constraint (11). 

A sequential quadratic programming method (SQP), (see for example [Pow83]), 
in which the search direction is a solution of a quadratic programming problem was adopted. 
The NAG Fortran Library Routine E04UCF actually implements the above algorithm and 
it was used to obtain the numerical results of the case study chapter. 

Notice that SQP is a gradient search method which is a local method. If the initial guess 
x° of the algorithm corresponds to an input vector u° that is in U aC c then the algorithm is 
going to converge to x* corresponding to u* E U acc • Similarly, if the initial guess x° of the 
algorithm corresponds to an input vector u° that is in Udec then the algorithm is going to 
converge to x* corresponding to u m E Udec • *^ n efficient method to obtain initial, and very 
close to optimal guesses either in U acc or in U ( i ec ls provided by the Minimum Interference 
Strategy (MIS) [KS91]. The Accelerating MIS provides a first guess x° E W acc , while the 
Decelerating MIS provides a first guess x° E £4f ec - 

SQP requires the derivatives of both the cost criterion and the constraints, w.r.t x . An 
analytical form of those derivatives can be easily obtained except for constraint (45) that 
involves the distance function This is well known to be continuous but not contin- 

uously differentiable [GJ85], A way to bypass this difficulty is to predict the “singularity” 
points where the derivative is discontinuous and interpolate locally with a sigmoid function. 
This approach was used in the case study and did not create any numerical problems. 
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The algorithm is for the Optimal Control Strategy is: 

OCS Algorithm 

STEPl : Find an initial input guess u° = {m®, € U a cc and the corresponding 

time and velocity trajectories t° ~ ...,*&}> v° = Formu- 

late the initial guess vector a: 0 . 

STEP2 : Feed z° into SQP and let it converge to x* cc , providing u* cc >t* cc and v* cc . 

STEPS : If J aC c = (Kcc( s n) — T) 2 i s ver y small, then an optimal solution has been found. 

STOP; 

Otherwise continue. 

STEP 4 : Find initial input guess u° = {uq. u®, ..., E 7/<iec and the corresponding 

time and velocity trajectories t° = {*?, *§> v ° = ( v i » u 2 5 — Formu- 

late the initial guess vector x°. 

STEPS : Feed x° into SQP and let it converge to x * dec , providing «3e c ^2cc anc * v dec 

STEP6 : If = (^cc( 5 ^) ” F) 2 is very small, then an optimal solution has been found. 

STOP; 

Otherwise compare J acc and J^ec and chose optimal solution. 

4 Simulation Results 

In this section a case study is presented. A mobile robot and a moving obstacle with 
geometric shapes, moving in the same environment 4. The shape of both the mobile robot 
and the moving obstacle is rectangle with dimensions 0.3m x 0.52m and 0.28m x 0.28m, 
respectively. The scenario is that when the mobile robot is about to start moving, it detects 
the obstacle, estimates its kinematic parameters 

xq = 4.0 m yo — 7.0 m 

v x = 0.04 m/sec v y — -0.075 ml sec 

a x = 0.094 m/sec 2 v y = —0.041 ml sec 2 

and predicts a collision under the current plan at t c = 6.67 sec . 

The mobile robot has parameters: 

Mass (Af) : 60 Kg. 

Inertia (I) : 32 Kg.m 2 
Maximum Accelerating Force (U\) : 140 N 
Minimum Decelerating Force ( U 2 ) : *60 N 
Maximum Velocity (V max ) : 8 m/sec 2 
Wheel-floor friction coefficient ( rj ) : ~0.3 

has the task to go from configuration A to configuration B within T = 12.1753 sec. An off- 
line path planning stage is done and a path r(s) 0 < s < sj with total length sj « 14.60 m 
is computed. The parameters of r(s) are indicated on figure 4. 

Initial and final velocities are zero ( v a = vq = 0). 
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Figure 4: Environment with a mobile robot and a moving obstacle 

The NAG Fortran Library Routine E04UCF was used to solve the nonlinear program- 
ming problem. The whole segment was divided in 111 segments. The first guesses required 
for the accelerating OCS ( u £ U ac c) and the decelerating OCS ( u £ £/<f ec ), were given by 
AMIS and DMIS respectively [KS91]. 

The resulting velocity profiles from the Accelerating OCS and the Decelerating OCS are 
plotted with solid line on figures 5 and 6 respectively. 

The time functions for Accelerating OCS (£ a cc(s)), Decelerating OCS (tdec($)) and the 
nominal plan (^nom^) are plotted on figure 7. The motion time of the nominal plan is 
T = 12.1753 sec, while T acc = 12.1753 sec and T^ ec = 12.2782 sec . 

The optimal control formulation although mathematically sound suffers from computa- 
tional complexity that prevents its on-line use. However the initial guess provided by MIS 
is very close to optimal and therefore the convergence is speeded up. Issues related to a 
parallel asynchronous implementation of the above algorithm for multiprocessor environ- 
ments are of our immediate interest. Furthermore, the issue of sensitivity of solution to the 
estimates of the kinematic parameters of the moving object has to be investigated. Prelim- 
inary results show that the present algorithm can be updated very efficiently. This is a very 
attractive feature because the initial penalty of computation is not frequently repeated. 
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5: Velocity profile of Accelerating OCS ( — ), nominal velocity (....) and v max ( ) 


3.5 
3 

2.5 
2 

1.5 
1 

0.5 
0 

0 2 4 6 8 10 12 14 16 

s: distance along r(s) (m) 

Figure 6: Velocity profile of Decelerating OCS ( — ), nominal velocity (....) and v max ( ) 
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Figure 7: Time for Accel. OCS (- - -), Decel. OCS ( ) and the nominal plan ( — ) 


APPENDIX 


Proof of Lemma 

Since f(o),g(o) e C[0, 1] 


dM{ A) 1 [* 


d A 


= ^ [A • / + (T - A) • 5] % -{g- f)- do 


and 


d 2 M{\) 3 f s 


d A 2 


= 7 / [A • / + (1 - A) - g] f -(g - ff -do 
4 Jo 


From (a),(c) > 0, thus M{ A) is convex J. 

Method of Martensson 
In brief, this method suggests that a hard constraint of the form 


\ 


(46) 

(47) 


(48) 


G(x(s),.s) > 0 
can be arbitrarily closely approximated by 

p(x(s), u(s), s) = G^(x(s),u(s),s) + ari-G^~ 1 )(x(s),u(s),s) + ... + a/-G(x(s),s) > 0 (49) 
where 

G <0(x M , = ( 5o) 




dx 
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by choosing constants a, £ 3?, i = 1, . . . / to be the coefficients of a stable polynomial with 
real roots 

X 1 + ai ■ X 1 ^ 4- ... 4" Qi = 0. (51 ) 

The more stable is the polynomial the better is the approximation of the original constraints. 
Notice that because of the structure (5) of the state equations, function g( ., .) is going to 
be of the form 

g(x(s),u(s),s) = C(.r,s) + V(x,s) ■ u(s ) (52) 

The order / is chosen such that in (50) a factor containing u explicitly exists.l. 
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